/**
  ******************************************************************************
  * @file   
  * @brief  
  ******************************************************************************
  * @note

 */

/* Includes ------------------------------------------------------------------*/
#include "PID.h"
/* Definitions ---------------------------------------------------------*/

/* Private variables ---------------------------------------------------------*/

/* Global variables ---------------------------------------------------------*/

/*Functions------------------------------------------------------------------*/
void PID_init(PID_OP *PID,float step_h,float rate_r,float parameter_p,float parameter_i,float parameter_d,float i_uplimit,float i_downlimit,float uplimit,float downlimit){
  integrator_op *int_add = (integrator_op*)pvPortMalloc(sizeof(integrator_op));
	differentiator_op *diff_add = (differentiator_op*)pvPortMalloc(sizeof(differentiator_op));
	PID->diff=diff_add;
	PID->integ=int_add;
	PID->PID_P=parameter_p;
	PID->PID_I=parameter_i;
	PID->PID_D=parameter_d;
	PID->output_uplimit=uplimit;
	PID->output_downlimit=downlimit;
	systick_integrator_init(PID->integ,i_uplimit,i_downlimit);
	differentiator_init(PID->diff,rate_r,step_h);
}

void PID_control(PID_OP *PID,float err){
	systick_integrator(err,PID->integ);
	fhan(PID->diff,err);
	iteration_differentiator(PID->diff);
	PID->output_value=(PID->PID_P*err)+(PID->PID_I*PID->integ->Integral_value)+(PID->PID_D*PID->diff->x2_now);
	if(PID->output_value>PID->output_uplimit){
		PID->output_value=PID->output_uplimit;
	}
	else if(PID->output_value<PID->output_downlimit){
		PID->output_value=PID->output_downlimit;
	}
}
/* Exported function declarations --------------------------------------------*/
